import math
import os
import sys
import threading
import time
from copy import deepcopy

from scservo_sdk import *  # Uses SCServo SDK library

if os.name == 'nt':
    import msvcrt

    def getch():
        return msvcrt.getch().decode()

else:
    import sys
    import termios
    import tty
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)

    def getch():
        try:
            tty.setraw(sys.stdin.fileno())
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch

# Control table address

# Default setting
BAUDRATE = 115200           # SCServo default baudrate : 1000000
# DEVICENAME = 'COM3'    # Check which port is being used on your controller
DEVICENAME = '/dev/ttyAMA0'   

# Initialize PortHandler instance
portHandler = PortHandler(DEVICENAME)

# Initialize PacketHandler instance
packetHandler = sms_sts(portHandler)

# Open port
if portHandler.openPort():
    print("Succeeded to open the port")
else:
    print("Failed to open the port")

# Set port baudrate
if portHandler.setBaudRate(BAUDRATE):
    print("Succeeded to change the baudrate")
else:
    print("Failed to change the baudrate")


class Servo_task(threading.Thread):
    def __init__(self, servo_num, moving_speed, moving_acc) -> None:
        super(Servo_task, self).__init__()
        self.servo_num = servo_num
        self.moving_speed = moving_speed
        self.moving_acc = moving_acc
        self.groupSyncRead = GroupSyncRead(
            packetHandler, SMS_STS_PRESENT_POSITION_L, 11)
        self.snake_mode = 0
        self.goal_position = []
        for i in range(0, servo_num):
            self.goal_position.append(0)
        self.open_servo = False
        self.thread_lock = threading.Lock()
        self.thread_exit = False

    def set_position(self):
        for servo_id in range(1, self.servo_num):
            # Add SCServo#1~10 goal position\moving speed\moving accc value to the Syncwrite parameter storage
            servo_addparam_result = packetHandler.SyncWritePosEx(
                servo_id, self.goal_position[servo_id], self.moving_speed, self.moving_acc)
            if servo_addparam_result != True:
                print("[ID:%03d] groupSyncWrite addparam failed" % servo_id)

        # Syncwrite goal position
        scs_comm_result = packetHandler.groupSyncWrite.txPacket()
        if scs_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(scs_comm_result))

        # Clear syncwrite parameter storage
        packetHandler.groupSyncWrite.clearParam()

    def read_data(self):
        # Add parameter storage for SCServo#1~10 present position value
        for servo_id in range(1, self.servo_num):
            servo_addparam_result = self.groupSyncRead.addParam(servo_id)
            if servo_addparam_result != True:
                print("[ID:%03d] groupSyncRead addparam failed" % servo_id)

        servo_comm_result = self.groupSyncRead.txRxPacket()
        if servo_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(servo_comm_result))

        for servo_id in range(1, self.servo_num):
            # Check if groupsyncread data of SCServo#1~10 is available
            servo_data_result, servo_error = self.groupSyncRead.isAvailable(
                servo_id, SMS_STS_PRESENT_POSITION_L, 11)
            if servo_data_result == True:
                # Get SCServo#scs_id present position moving value
                servo_present_position = self.groupSyncRead.getData(
                    servo_id, SMS_STS_PRESENT_POSITION_L, 2)
                servo_present_speed = self.groupSyncRead.getData(
                    servo_id, SMS_STS_PRESENT_SPEED_L, 2)
                servo_present_moving = self.groupSyncRead.getData(
                    servo_id, SMS_STS_MOVING, 1)
                print("[ID:%03d] PresPos:%d PresSpd:%d" % (
                    servo_id, servo_present_position, packetHandler.scs_tohost(servo_present_speed, 15)))
            else:
                print("[ID:%03d] groupSyncRead getdata failed" % servo_id)
                continue
            if servo_error:
                print(packetHandler.getRxPacketError(servo_error))

        # Clear syncread parameter storage
        self.groupSyncRead.clearParam()

    def gamepad_control(self):
        pass

    def goal_calculate(self, now_time):
        for servo_index in range(0, self.servo_num):
            # 初始化状态
            if self.snake_mode == 0:
                self.goal_position[servo_index] = 2048
    
            # 翻滚步态
            if self.snake_mode == 1:
                b = 0.0
                A = 0.3
                c = 3.1415/2.0

                w1 = servo_index/13*2.5
                w2 = servo_index/13*2.5
                v1 = 0
                v2 = 0

                x1 = w1*time + v1*servo_index
                x2 = w2*time + v2*servo_index

                if(servo_index % 2 != 0):
                    self.goal_position[servo_index] = b + A*math.sin(x1)
                else:
                    self.goal_position[servo_index] = b + A*math.sin(x2+c)

    def run(self):
        start_time = time.time()
        while not self.thread_exit:
            if(self.open_servo == True):
                now_time = time.time() - start_time
                self.goal_calculate(now_time)
                self.set_position()
                time.sleep(0.1)
            else:
                time.sleep(0.1)
